/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: JointGroupCtrlServer.hpp,v 1.9 2009-09-23 11:56:14 morsko Exp $
 *
 * \file
 * \brief MaCI - JointGroupCtrl Server interface
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 * \todo Maybe some Getters(get speed, getposition)
 */
#ifndef _MACI_INTERFACE_JOINTGROUPCTRLSERVER_HPP_
#define _MACI_INTERFACE_JOINTGROUPCTRLSERVER_HPP_

#include "MaCI.hpp"
#include "JointGroupCtrlData.hpp"
#include "thread.hpp"
#include "sync.hpp"
#include "binbag.h"
#include <list>

// Forward declare gimi::GIMI
namespace gimi {
  class GIMI;
  class GIMIMessage;
}


namespace MaCI{
  namespace JointGroupCtrl{

    typedef std::vector<TJointInformation_Payload> TJointInformationArray;
    /** MaCI - JointGroupCtrl Server interface
     * 
     * This class contains the methods and types for providing
     * the JointGroupCtrl service in the GIMnet.
     */
    class CJointGroupCtrlServer : public MaCI::CMaCI,
				  private gim::CThread,
				  private gim::CSync
    {
    public:

      /** Constructor.
       * 
       * Constructor of the JointGroupCtrlServer. This requires;
       * a pointer to Initialized and connected MaCICtrlServer, minor
       * number of this interface (Used as GIMI minor number)
       *
       * @param[in] aMaCICtrlServerPtr Pointer to MaCICtrlServer used for communication
       * @param[in] aInterfaceMinor Interface minor number. Usually
       * used to indicate the instance of devices of same
       * type.
       */
      CJointGroupCtrlServer(MaCICtrl::CMaCICtrlServer *aMaCICtrlServerPtr, const int aInterfaceMinor);

      /** Destructor.
       */
      ~CJointGroupCtrlServer();

      /** Opens JointGroupCtrl Server
       *
       * Opens the JointGroupCtrl Server,  basicly registers device to GIMI.
       * NOTE: Before opening you MUST set the joint group info! 
       *  
       * @return                Returns a value of type EMaCIError. 
       *                        
       * \see EMaCIError
       */
      MaCI::EMaCIError DoOpen(void);

      /** Close device.
       *
       * Closes the device. Closes all GIMI registrations and queues.
       *
       * @return                Returns a value of type EMaCIError.
       *
       * \see EMaCIError
       */
      MaCI::EMaCIError DoClose(void);

      /** Wait for a Command.
       *
       * This function waits until new command arrives or timeout occurs.
       * If there is unhandled commands since last read, the next
       * in queue is returned. If you want to clear the queue between calls,
       * use ClearCommandBuffer() function.
       *
       * When a command is succesfully returned, the user can inspect
       * the current command through the GetCommand() call of
       * class CJointGroupCtrlData. Then, according to command and its
       * specification, additional datafields may be present, they
       * can be queried by similar Get... functions. See the 
       * description of the data encoder/decoder module CJointGroupCtrlData.
       *
       * No need to implement the GetJointInformation-command. It is done by the 
       * server automatically.
       * 
       * \see ClearCommandBuffer()
       * \see CIOData
       * \see MaCI::EMaCIError
       * 
       * @param[out] aData      Reference to IOData. This will
       *                        be filled with incoming command when
       *                        this call is completed succesfully.
       * @param[out] aInfo      Reference to replyInfo. Use this replyInfo 
       *                        to SendReply-function when you have to send 
       *                        a Reply.
       * @param[in] aTimeout_ms Timeout to wait until operation fails
       *                        for timeout.
       * @param[in] aSkipQueued If this value is set to 'true', the
       *                        function will return the latest command
       *                        which has the same command and joint index
       *                        as the oldest command. All the older commands
       *                        which are the same(with same joint index) will 
       *                        be skipped and deleted from the command queue. 
       *                        e.g. is axis 1 is commanded with speed 1 and 2 
       *                        and after that axis 2 with speed 1. First time 
       *                        the function will return "SetJointSpeed" with 
       *                        axis 1 and speed 2 and second time the 
       *                        "SetJointSpeed" with axis 2 with speed 1.
       *                        If this is set to 'false', all commands are 
       *                        received in order. If parameter is undefined, 
       *                        default is 'false'. 
       *
       * @return                'KMaCIOK'  when succesfully received
       *                        a new command. KMaCITimeout when operation
       *                        timed out before a command was received.
       *                        Other errors returned when appropriate.
       *                        (See the MaCI::EMaCIError for description)
       * \see EMaCIError
       */
      MaCI::EMaCIError WaitCommand(CJointGroupCtrlData &aData, 
				   TReplyInfo &aInfo,
                                   const unsigned int aTimeout_ms,
				   bool aSkipQueued = false);

      /** Sends reply message to client
       *
       * This function sends synchronous reply message to client.
       *
       * @param[in] aData       Reference to CJointGroupCtrlData. This data will be sent to
       *                        client for reply
       * @param[in] aInfo       Reference to replyInfo. Get this replyinfo from
       *                        WaitCommand.
       * @return                'KMaCIOK' when reply message succesfully sent.
       *                        Other errors returned when appropriate.
       *                        (See the MaCI::EMaCIError for description)
       * \see EMaCIError
       */
      MaCI::EMaCIError SendReply(CJointGroupCtrlData &aData,
				 TReplyInfo &aInfo);

       /** Clear the incoming commands buffer.
       *
       * This functions clears the accumulated command buffer. This
       * function call may be required if the server for some (weird?)
       * reason wants to skip unprocessed commands. Per default the incoming
       * 'Set' commands are all queued until processed. (This ensures
       * that critical commands don't get lost)
       */
      void ClearCommandBuffer(void);

      /**
       * Sets information array about all joints. You must set it before 
       * Opening the server. This is because client must know which joint 
       * correspond each jointIndex number. Also information how it can be 
       * commanded is pretty useful :)
       *
       * @param[in] aArray        JointInformation array
       * \see TJointInformatioArray
       * \see TJointInformation_Payload
       */
      void SetJointGroupInfo(TJointInformationArray &aArray);

      /**
       * Sends JointGroupCtrl event to service subscribers.
       * The CJointGroupCtrlData must contain KCommandEvent-Command and the structures
       * set at the SetEventTypes-function
       *
       * @param[in] aData         CJointGroupCtrlData container containing KCommandEvent-Command 
       *                          and the structures set at the SetEventTypes-function
       *                          
       */
      bool SendEvent(const CJointGroupCtrlData &aData);

      /**
       * Set the types which are sent by event. you must set this before opening the
       * Server if you want to send some information as events.
       * If you are using events, you can ignore the GetPosition/Speed/Force commands
       * (Depending on the eventTypes)
       * and have not to handle these messages.
       *
       * @param[in] aEventTypes Reference to TEventTypes. This data will be set, and
       *                        client will ask it when connecting to server.
       * @return                true if success, false if fails
       */
      bool SetEventTypes(TEventTypes &aEventTypes);

    private:
      int ThreadFunction(const int aThreadNumber); ///< Incoming data handler function
      int ProcessMessage(const gimi::GIMIMessage &msg); ///< Incoming data processor function
      volatile bool iServerActive; ///< Flag indicating whether the server is active.

      std::vector<const CJointGroupCtrlData *> iJointGroupCtrlCommandArray; ///< Array of unprocessed commands.
      std::vector<const TReplyInfo *> iReplyDataArray; ///< Array of unprocessed commands.
      TJointInformationArray iJointInformationArray;
      bool iJointInformationSet;
      TEventTypes iEventTypes;
      
      //For testing
    
    
      int cmdCount;

    };

  }

}

#endif ///< _MACI_INTERFACE_JOINTGROUPCTRLSERVER_HPP_

